package org.nashua.tt151;

import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.SimpleRobot;
import java.io.IOException;
import org.nashua.tt151.libraries.Controller.DualAction;

public class ClimbTester extends SimpleRobot {
    DualAction joy = new DualAction(1);
    Jaguar sh1 = new Jaguar(7);
    Jaguar sh2 = new Jaguar(8);
    Jaguar cl1 = new Jaguar(5);
    Jaguar cl2 = new Jaguar(6);
    double sh = 0;
    Dash dash = null;
    private int failedCount;
    private boolean tryingToConnect = false;
    private void connectToDashboard() {
        if (tryingToConnect) {
            return;
        }
        tryingToConnect = true;
        new Thread() {
            public void run() {
                try {
                    if (tryingToConnect && dash==null) {
                        dash = new Dash("10.1.51.5", new Dash.ConnectionListener() {
                            public void onConnect() {
                                System.out.println("[Connected]");
                            }
                            public void onDisconnect() {
                                System.out.println("[Disconnected]");
                                dash = null;
                            }
                            public void onDataReceived(String msg) {
                                System.out.println("[MSG "+msg+"]");
                            }
                        });
                    }
                } catch (IOException ex) {
                    System.err.println("[ERR Failed to connect to dashboard: "+ex.getMessage()+"]");
                }
                tryingToConnect = false;
            }
        }.start();
    }
    public void disabled() {
        if (dash==null && !tryingToConnect) {
            connectToDashboard();
        }
    }
    public void robotInit() {
        connectToDashboard();
        failedCount = 0;
        new java.util.Timer().scheduleAtFixedRate(new java.util.TimerTask(){
            public void run() {
                if (dash!=null) {
                    try {
                        dash.sendPWM((int)(cl1.get()*128+128), "Climb Left", cl1.getModuleNumber(), cl1.getChannel(), Dash.PWMType.JAGUAR);
                        dash.sendPWM((int)(cl2.get()*128+128), "Climb Right", cl2.getModuleNumber(), cl2.getChannel(), Dash.PWMType.JAGUAR);
                        dash.sendPWM((int)(sh1.get()*128+128), "Shooter 1", sh1.getModuleNumber(), sh1.getChannel(), Dash.PWMType.JAGUAR);
                        dash.sendPWM((int)(sh2.get()*128+128), "Shooter 2", sh2.getModuleNumber(), sh2.getChannel(), Dash.PWMType.JAGUAR);
                        failedCount = 0;
                    } catch (Exception ex) {
                        System.err.println("Failed to update dashboard: "+ex.getMessage());
                        failedCount++;
                        if (failedCount<=10) {
                            failedCount = 0;
                            dash = null;
                            connectToDashboard();
                        }
                    }
                } else {
                    failedCount = 0;
                    connectToDashboard();
                }
            }
        }, 1, 100);
    }
    public void operatorControl() {
        sh = 0;
        while (isEnabled() && isOperatorControl()) {
            joy.queryButtons();
            double val1 = joy.getLeftY();
            double val2 = joy.getRightY();
            cl1.set(-val1);
            cl2.set(val2);
            if (joy.wasReleased(5)) {
                sh -= 0.1;
            }
            if (joy.wasReleased(6)) {
                sh += 0.1;
            }
            if (joy.wasReleased(7)) {
                sh -= 1;
            }
            if (joy.wasReleased(8)) {
                sh += 1;
            }
            sh1.set(sh/12.0);
            sh2.set(sh/12.0);
        }
    }
}
